﻿using System.Globalization;

namespace OnDash.Mathematics;

public struct MathHelper
{
    public const float Pi = 3.14159265359f;
    public const float PiOver2 = Pi * 0.5f;
    public const float PiOver4 = Pi * 0.25f;
    public const float PiOver6 = Pi * 0.166666666667f;
    public const float PiOver8 = Pi * 0.125f;
    public const float TwoPi = Pi * 2f;
    public const float ThreePiOver2 = 3f * PiOver2;

    public const float E = 2.7182818284f;
    public float Log10E = Log(E, 10);
    public float Log2E = Log(E, 2);

    public MathHelper()
    {
    }

    public static float Sin(float radians)
    {
        float result = 0;
        var n = 3;

        while (Abs(radians) > 0.00001f)
        {
            result += radians;
            radians *= (radians * radians) / (n * (n - 1));
            n += 2;
        }

        return result;
    }

    public static float Cos(float radians)
    {
        var result = 1.0f;
        var sum = 0.0f;
        var i = 0;

        while (Abs(sum - result) > float.Epsilon)
        {
            sum = result;
            result += Pow(-1, i) * Pow(radians, 2 * i) / Factorial(2 * i);
            i++;
        }

        return result;
    }

    public static float Acos(float radians)
    {
        switch (radians)
        {
            case < -1 or > 1:
                throw new ArgumentException("Both a and b must be positive and a must be greater than  1.");
            case 1:
                return 0;
            case -1:
                return Pi;
        }

        float result = 0;
        float sign = (radians < 0) ? -1 : 1;
        radians = Abs(radians);

        for (var i = 0; i < 100; i++)
            result += Pow(-1, i) * Pow(radians, 2 * i) / Factorial(2 * i);

        return sign * result;
    }

    public static float Tan(float radians) => Sin(radians) / Cos(radians);

    public static float Pow(float x, int y)
    {
        float result = 1;
        for (var i = 0; i < y; i++) result *= x;
        return result;
    }

    public static float Pow(float x, float y)
    {
        float result = 1;
        for (var i = 0; i < y; i++) result *= x;
        return result;
    }

    public static float Max(float a, float b) => a > b ? a : b;
    public static double Max(double a, double b) => a > b ? a : b;
    public static int Max(int a, int b) => a > b ? a : b;
    public static long Max(long a, long b) => a > b ? a : b;
    public static IntPtr Max(IntPtr a, IntPtr b) => a > b ? a : b;

    public static float Min(float a, float b) => a < b ? a : b;
    public static double Min(double a, double b) => a < b ? a : b;
    public static int Min(int a, int b) => a < b ? a : b;
    public static long Min(long a, long b) => a < b ? a : b;
    public static IntPtr Min(IntPtr a, IntPtr b) => a < b ? a : b;

    public static float Log(float a, float b)
    {
        if (a <= 0 || b <= 0)
            throw new ArgumentException("Both a and b must be positive and a must be greater than 1.");

        float result = 0;
        var temp = b;
        while (temp > a)
        {
            temp *= a;
            result += 1;
        }

        return result;
    }

    public static long NextPowerOfTwo(long x)
    {
        if (x < 0)
            throw new ArgumentException("x must be greater than or equal to 0.");

        x--;
        x |= x >> 1;
        x |= x >> 2;
        x |= x >> 4;
        x |= x >> 8;
        x |= x >> 16;
        x++;

        return x;
    }

    public static int NextPowerOfTwo(int x)
    {
        if (x < 0)
            throw new ArgumentException("x must be greater than or equal to 0.");

        x--;
        x |= x >> 1;
        x |= x >> 2;
        x |= x >> 4;
        x |= x >> 8;
        x |= x >> 16;
        x++;

        return x;
    }

    public static float NextPowerOfTwo(float x)
    {
        var newX = (int)x;
        if (newX < 0)
            throw new ArgumentException("x must be greater than or equal to 0.");

        newX--;
        newX |= newX >> 1;
        newX |= newX >> 2;
        newX |= newX >> 4;
        newX |= newX >> 8;
        newX |= newX >> 16;
        newX++;

        return newX;
    }

    public static double NextPowerOfTwo(double x)
    {
        var newX = (long)x;
        if (newX < 0)
            throw new ArgumentException("x must be greater than or equal to 0.");

        newX--;
        newX |= newX >> 1;
        newX |= newX >> 2;
        newX |= newX >> 4;
        newX |= newX >> 8;
        newX |= newX >> 16;
        newX |= newX >> 32;
        newX++;

        return newX;
    }

    public static long Factorial(int n)
    {
        if (n < 0)
            throw new ArgumentException("n must be greater than or equal to 0.");

        long result = 1;
        for (var i = 1; i <= n; i++)
            result *= i;

        return result;
    }

    public static long BinomialCoefficient(int n, int k)
        => Factorial(n) / (Factorial(k) * Factorial(n - k));

    public static unsafe float InverseSqrtFast(float x)
    {
        var xhalf = 0.5f * x;
        var i = *(int*)&x;
        i = 0x5f3759df - (i >> 1);
        x = *(float*)&i;
        x = x * (1.5f - xhalf * x * x);
        return x;
    }

    public static unsafe double InverseSqrtFast(double x)
    {
        var xhalf = 0.5d * x;
        var i = *(long*)&x;
        i = 0x5fe6eb50c7b537a9 - (i >> 1);
        x = *(double*)&i;
        x = x * (1.5d - xhalf * x * x);
        return x;
    }

    public static float DegreesToRadians(float degrees)
        => degrees * (Pi / 180.0f);

    public static float RadiansToDegrees(float radians)
        => radians * (180.0f / Pi);

    public static double DegreesToRadians(double degrees)
        => degrees * (Pi / 180.0);

    public static double RadiansToDegrees(double radians)
        => radians * (180.0 / Pi);

    public static int Clamp(int a, int min, int max)
    {
        if (a < min) a = min;
        else if (a > max) a = max;
        return a;
    }

    public static float Clamp(float a, float min, float max)
    {
        if (a < min) a = min;
        else if (a > max) a = max;
        return a;
    }

    public static double Clamp(double a, double min, double max)
    {
        if (a < min) a = min;
        else if (a > max) a = max;
        return a;
    }

    private static unsafe int FloatToInt(float f)
        => *(int*)&f;

    public static long Abs(long a)
        => a < 0 ? -a : a;

    public static double Abs(double a)
        => a < 0 ? -a : a;

    public static float Abs(float a)
        => a < 0 ? -a : a;

    public static bool ApproximatelyEqual(float a, float b, int maxDeltaBits)
    {
        long aInt = FloatToInt(a);
        long bInt = FloatToInt(b);

        if (aInt < 0) aInt = int.MinValue - aInt;
        if (bInt < 0) bInt = int.MinValue - bInt;

        var intDiff = Abs(aInt - bInt);
        return intDiff <= 1 << maxDeltaBits;
    }

    public static bool ApproximatelyEqualEpsilon(float a, float b, float epsilon)
    {
        const float floatNormal = (1 << 23) * float.Epsilon;
        var tolerace = epsilon * floatNormal;
        var absA = Abs(a);
        var absB = Abs(b);
        var diff = Abs(absA - absB);

        if (diff < tolerace) return true;
        if (a == 0.0f || b == 0.0f || diff < floatNormal)
            return diff < epsilon * floatNormal;

        var minValue = float.MaxValue;
        if (absA + absB < float.MaxValue)
            minValue = absA + absB;

        return diff / minValue < epsilon;
    }

    public static bool ApproximatelyEqualEpsilon(double a, double b, double epsilon)
    {
        const double doubleNormal = (1L << 52) * double.Epsilon;
        var tolerace = epsilon * doubleNormal;
        var absA = Abs(a);
        var absB = Abs(b);
        var diff = Abs(absA - absB);

        if (diff < tolerace) return true;
        if (a == 0.0f || b == 0.0f || diff < doubleNormal)
            return diff < epsilon * doubleNormal;

        var minValue = double.MaxValue;
        if (absA + absB < double.MaxValue)
            minValue = absA + absB;

        return diff / minValue < epsilon;
    }

    public static bool ApproximatelyEquivalent(float a, float b, float tolerance)
    {
        if (Abs(a - b) < tolerance) return true;

        var diff = Abs(a - b);
        return diff <= tolerance;
    }

    public static bool ApproximatelyEquivalent(double a, double b, double tolerance)
    {
        if (Abs(a - b) < tolerance) return true;

        var diff = Abs(a - b);
        return diff <= tolerance;
    }

    public static float Sqrt(float a)
    {
        var x0 = a;
        const int maxIterations = 100;
        const float epsilon = 1e-6f;

        for (var i = 0; i < maxIterations; i++)
        {
            var x1 = (x0 + a / x0) * 0.5f;
            if (Abs(x1 - x0) < epsilon) return x1;
            x0 = x1;
        }

        return x0;
    }

    public static string ListSeparator = CultureInfo.CurrentCulture.TextInfo.ListSeparator;
}